/* debugger.c 
**
** Copyright 2006, Brian Swetland.  All rights reserved.
** See provided LICENSE file or http://frotz.net/LICENSE for details.
*/

#include "at91sam7s.h"
#include "devices.h"
#include "usb.h"

#include "jtag_protocol.h"
#include "jtag.h"

#define ARM_NOP             0xE1A00000

#define MODE_UNKNOWN 0
#define MODE_RESET 1
#define MODE_DEBUG 2
#define MODE_ICE 3
#define MODE_RESTART 4

static unsigned mode = MODE_UNKNOWN;


void goto_debug()
{
    if(mode == MODE_UNKNOWN) jtag_reset();
    jtag_select_debug();
    mode = MODE_DEBUG;
}

void goto_ice()
{
    if(mode == MODE_UNKNOWN) jtag_reset();
    jtag_select_ice();
    mode = MODE_ICE;
}

void goto_restart()
{
    if(mode == MODE_UNKNOWN) jtag_reset();
    jtag_select_restart();
    mode = MODE_RESTART;
}

#define need_debug() if(mode != MODE_DEBUG) goto_debug()
#define need_ice() if(mode != MODE_ICE) goto_ice()


int execute(unsigned *cmd, unsigned *res, int ccount)
{
    unsigned *cmd_last = cmd + ccount;
    unsigned *res_first = res;
    unsigned count, n;
    
    while(cmd < cmd_last) {
        unsigned opcode = *cmd++;
        switch(opcode & 0xff){
        case OP_RESET:
            jtag_reset();
            mode = MODE_RESET;
            break;
        case OP_ID_RD:
            *res++ = jtag_get_id();
            mode = MODE_RESET;
            break;
        case OP_ICE_RD:
            need_ice();
            *res++ = jtag_ice_rd(*cmd++);
            break;
        case OP_ICE_WR:
            need_ice();
            count = opcode >> 8;
            while(count-- > 0){
                jtag_ice_wr(cmd[0], cmd[1]);
                cmd += 2;
            }
            break;
        case OP_ICE_WAIT:
            need_ice();
            for(count = 0; count < 100; count++){
                n = jtag_ice_rd(cmd[0]);
                if((n & cmd[1]) == cmd[2]) break;
            }
            if(count == 100) return -ERROR_TIMEOUT;
            cmd += 3;
            break;
        case OP_RESTART:
            goto_restart();
            break;
        case OP_DBG_RD:
            need_debug();
            count = opcode >> 8;
            while(count-- > 0){
                *res++ = jtag_dbg_io(ARM_NOP, 0);
            }
            break;
        case OP_DBG_WR:
            need_debug();
            count = opcode >> 8;
            while(count-- > 0){
                jtag_dbg_io(*cmd++, 0);
            }
            break;
        case OP_DBG_WR_BKPT:
            need_debug();
            jtag_dbg_io(ARM_NOP, 1);
            break;
        case OP_EXEC:
            need_debug();
            jtag_dbg_io(ARM_NOP, 0);
            jtag_dbg_io(ARM_NOP, 1);
            jtag_dbg_io(*cmd++, 0);
            jtag_dbg_io(ARM_NOP, 0);
            goto_restart();
            need_ice();
            for(count = 0; count < 100; count++){
                if((jtag_ice_rd(ICE_DEBUG_STATUS) & 9) == 9) break;
            }
            if(count == 100) return -ERROR_TIMEOUT;
            break;
        case OP_RESET_0:
            jtag_srst(0);
            break;
        case OP_RESET_1:
            jtag_srst(1);
            break;
        default:
            return -ERROR_BADOP;
        }
    }
    return res - res_first;
}

void usb_status(unsigned online)
{
}

static unsigned *commands = (unsigned*) 0x202000;
static unsigned *results = (unsigned*)  0x203000;

#define STATE_IDLE      0
#define STATE_DATA_IN   1
#define STATE_STATUS    2
#define STATE_DATA_OUT  3

static unsigned state = STATE_IDLE;

static unsigned status[2];
static unsigned count = 0;
static unsigned *pointer = 0;
static unsigned expected = 0;


unsigned usb_bulk_send(unsigned char **ptr)
{
    unsigned n;
    
    switch(state){
    case STATE_STATUS:
        *ptr = (unsigned char*) status;
        if(count > 0) {
            state = STATE_DATA_OUT;
        } else {
            state = STATE_IDLE;
        }
        return 8;

    case STATE_DATA_OUT:
        n = (count > 64) ? 64 : count;
        *ptr = (unsigned char*) pointer;
        pointer += (n / 4);
        count -= n;
        if(count == 0) {
            state = STATE_IDLE;
        }
        return n;

    default:
        return 0;
    }
}

unsigned usb_bulk_recv(void *_data, unsigned len)
{
    unsigned *data = (unsigned*) _data;
    unsigned n;
    
    len /= 4;

    switch(state){
    case STATE_STATUS:
    case STATE_DATA_OUT:
            /* if we get a new command while
               responding to a previous one,
               abort the response */
        state = STATE_IDLE;
        count = 0;
            /* fall through */
        
    case STATE_IDLE:
        status[1] = ERROR_HEADER;
        if(len < 2) goto error;
        n = *data++;
        if(n != JTAG_COMMAND) goto error;
        expected = *data++;
        if(expected > 1024) goto error;
        state = STATE_DATA_IN;
        pointer = commands;
        count = 0;
        len -= 2;
            /* fall through */
        
    case STATE_DATA_IN:
        for(n = 0; n < len; n++){
            *pointer++ = *data++;
        }
        count += len;
        if(count >= expected){
            int r = execute(commands, results, expected);
            if(r < 0) {
                status[1] = -r;
                goto error;
            }
            state = STATE_STATUS;
            status[0] = JTAG_REPLY;
            status[1] = r;
            pointer = results;
            count = r * 4;
            return 1;
        }
        return 0;

    default:
        return 0;
    }
    
error:
    state = STATE_STATUS;
    status[0] = JTAG_ERROR;
    count = 0;
    return 1;
}
